
#include <BMesh>
#include <BSphere>
#include <BCube>
#include <BCylinder>
#include <BGeoSphere>
#include <BCone>
#include <BCapsule>
#include <BTerrain>
#include <BSurface>
#include <BBillboard>
#include <BGridPlane>

#include "ICubeCubeAlgorithm.h"
#include "ISphereSphereAlgorithm.h"
#include "ISoftBodyInternals.h"
#include "ISoftSoftAlgorithm.h"
#include "ISoftConcaveAlgorithm.h"
#include "ISoftRigidAlgorithm.h"
#include "IConvexPlaneAlgorithm.h"
#include "IConvexConvexAlgorithm.h"
#include "IConvexConcaveAlgorithm.h"

#include "member_BScene.h"
#include "member_BGroup.h"
#include "member_BShape.h"
#include "member_BNode.h"
#include "member_BBody.h"
#include "member_BCamera.h"
#include "member_BMesh.h"
#include "member_BGeometry.h"
#include "member_BObject.h"
#include "member_BRestraint.h"

using namespace BWE;

member_BScene::member_BScene(BScene* scene)
{
	boss = scene;
	refresh = false;
	dirtySpace = false;

	fixedStep = BReal(1) / 60;
	paused = true;
	tick = 0;
	totalTime = 0;

	softInfo.broadphase = &broadphase;
	softInfo.sparsesdf.reset();
	softInfo.air_density = (BReal)1.2;
	softInfo.water_density = 0;
	softInfo.water_offset = 0;
}
member_BScene::~member_BScene()
{

}

void member_BScene::update(BReal time)
{
	for (int i = 0; i < shapes.size(); i++)
	{
		BShape* shape = shapes[i];
		if (shape_member(shape)->dirty)
		{
			shape->flush();
		}
	}
	for (int i = 0; i < nodes.size(); i++)
	{
		BNode* node = nodes[i];
		BShapeHolderSet& shapes = node_member(node)->shapes;
		for (int i = 0; i < shapes.size(); i++)
		{
			BShape* shape = shapes[i];
			if (shape_member(shape)->dirty)
			{
				shape->flush();
				node_member(node)->dirtyBox = true;
				node_member(node)->dirtySpace = true;
			}
		}
		if (node_member(node)->dirtySpace)
		{
			node->space();
		}
	}
	for (int i = 0; i < groups.size(); i++)
	{
		BGroup* group = groups[i];
		updateGroup(group, time);
	}
	if (dirtySpace)
	{
		boss->space();
	}
	if (refresh)
	{
		for (int i = 0; i < cameras.size(); i++)
		{
			BCamera* camera = cameras[i];
			camera_member(camera)->refresh = true;
		}
		boss->emit(Signal_Freshed);
		refresh = false;
	}
	boss->update(time);
}
void member_BScene::updateGroup(BGroup* group, BReal time)
{
	for (int i = 0; i < group->shapeCount(); i++)
	{
		BShape* shape = group->shape(i);
		if (shape_member(shape)->dirty)
		{
			shape->flush();
		}
	}
	for (int i = 0; i < group->nodeCount(); i++)
	{
		BNode* node = group->node(i);
		BShapeHolderSet& shapes = node_member(node)->shapes;
		for (int i = 0; i < shapes.size(); i++)
		{
			BShape* shape = shapes[i];
			if (shape_member(shape)->dirty)
			{
				shape->flush();
				node_member(node)->dirtyBox = true;
				node_member(node)->dirtySpace = true;
			}
		}
		if (node_member(node)->dirtySpace)
		{
			node->space();
		}
	}
	for (int i = 0; i < group->childCount(); i++)
	{
		BGroup* child = group->child(i);
		updateGroup(child, time);
	}
	if (group->enabled())
	{
		group_member(group)->update(time);
	}
}

void member_BScene::freshOctTree()
{
	BSpace octSpace = sceneOctTree->space();
	const BSpace& space = boss->space();
	if (sceneOctTree->empty() || !octSpace.contain(space))
	{
		sceneOctTree->reset(space * 2.0f);
		shapeSpaceMap.reserve(shapes.size());
		nodeSpaceMap.reserve(nodes.size());
		for (int i = 0; i < shapes.size(); i++)
		{
			BShape* shape = shapes[i];
			SceneOctTreeItem item = sceneOctTree->create(shape->space());
			item->shapes.append(shape);
			shapeSpaceMap[shape] = item;
		}
		for (int i = 0; i < nodes.size(); i++)
		{
			BNode* node = nodes[i];
			SceneOctTreeItem item = sceneOctTree->create(node->space());
			item->nodes.append(node);
			nodeSpaceMap[node] = item;
		}
		for (int i = 0; i < groups.size(); i++)
		{
			BGroup* group = groups[i];
			freshGroupOctTree(group);
		}
	}
}
void member_BScene::freshGroupOctTree(BGroup* group)
{
	for (int i = 0; i < group->shapeCount(); i++)
	{
		BShape* shape = group->shape(i);
		SceneOctTreeItem item = sceneOctTree->create(shape->space());
		item->shapes.append(shape);
		shapeSpaceMap[shape] = item;
	}
	for (int i = 0; i < group->nodeCount(); i++)
	{
		BNode* node = group->node(i);
		SceneOctTreeItem item = sceneOctTree->create(node->space());
		item->nodes.append(node);
		nodeSpaceMap[node] = item;
	}
	for (int i = 0; i < group->childCount(); i++)
	{
		BGroup* child = group->child(i);
		freshGroupOctTree(child);
	}
}

void member_BScene::collectShapes(SceneOctTreeItem& item, BShapeArray& shapes)
{
	shapes.append(item->shapes);
	for (int i = 0; i < 8; i++)
	{
		SceneOctTreeItem lower = item.lower(i);
		if (lower.empty())
			continue;
		collectShapes(lower, shapes);
	}
}
void member_BScene::collectNodes(SceneOctTreeItem& item, BNodeArray& nodes)
{
	nodes.append(item->nodes);
	for (int i = 0; i < 8; i++)
	{
		SceneOctTreeItem lower = item.lower(i);
		if (lower.empty())
			continue;
		collectNodes(lower, nodes);
	}
}
void member_BScene::collectBodies(SceneOctTreeItem& item, BBodyArray& bodies)
{
	bodies.append(item->bodies);
	for (int i = 0; i < 8; i++)
	{
		SceneOctTreeItem lower = item.lower(i);
		if (lower.empty())
			continue;
		collectBodies(lower, bodies);
	}
}
void member_BScene::collectLights(SceneOctTreeItem& item, BLightArray& lights)
{
	lights.append(item->lights);
	for (int i = 0; i < 8; i++)
	{
		SceneOctTreeItem lower = item.lower(i);
		if (lower.empty())
			continue;
		collectLights(lower, lights);
	}
}
void member_BScene::collectCameras(SceneOctTreeItem& item, BCameraArray& cameras)
{
	cameras.append(item->cameras);
	for (int i = 0; i < 8; i++)
	{
		SceneOctTreeItem lower = item.lower(i);
		if (lower.empty())
			continue;
		collectCameras(lower, cameras);
	}
}

void member_BScene::attachGroup(BGroup* group)
{
	for (int i = 0; i < group_member(group)->nodes.size(); i++)
	{
		BNode* node = group_member(group)->nodes[i];
		if (BBody* body = dynamic_cast<BBody*>(node))
		{
			if (BSoftBody* softBody = dynamic_cast<BSoftBody*>(body))
			{
				softBodies.insert(softBody);
			}
			broadphase.createProxy(body);
			bodies.insert(body);
		}
		else if (BLight* light = dynamic_cast<BLight*>(node))
		{
			lights.insert(light);
		}
		else if (BCamera* camera = dynamic_cast<BCamera*>(node))
		{
			cameras.insert(camera);
		}
	}
	for (int i = 0; i < group_member(group)->children.size(); i++)
	{
		BGroup* child = group_member(group)->children[i];
		attachGroup(child);
	}
}
void member_BScene::detachGroup(BGroup* group)
{
	for (int i = 0; i < group_member(group)->nodes.size(); i++)
	{
		BNode* node = group_member(group)->nodes[i];
		if (BBody* body = dynamic_cast<BBody*>(node))
		{
			disconnect(body, Signal_Changed);
			if (BSoftBody* softBody = dynamic_cast<BSoftBody*>(body))
			{
				softBodies.remove(softBody);
			}
			broadphase.removeProxy(body);
			bodies.remove(body);
		}
		else if (const BLight* light = dynamic_cast<const BLight*>(node))
		{
			lights.remove(light);
		}
		else if (const BCamera* camera = dynamic_cast<const BCamera*>(node))
		{
			cameras.remove(camera);
		}
	}
	for (int i = 0; i < group_member(group)->children.size(); i++)
	{
		BGroup* child = group_member(group)->children[i];
		detachGroup(child);
	}
}

IAlgorithm* member_BScene::createAlgorithm(BShape* shape0, BShape* shape1)
{
	if (dynamic_cast<BCube*>(shape0) && dynamic_cast<BCube*>(shape1))
		return new ICubeCubeAlgorithm();

	if (dynamic_cast<BSphere*>(shape0) && dynamic_cast<BSphere*>(shape1))
		return new ISphereSphereAlgorithm();

	if (dynamic_cast<ISoftBodyCollisionShape*>(shape0) && dynamic_cast<ISoftBodyCollisionShape*>(shape1))
		return new ISoftSoftAlgorithm();

	if (dynamic_cast<ISoftBodyCollisionShape*>(shape0) && dynamic_cast<BMesh*>(shape1))
		return new ISoftConcaveAlgorithm();

	if (dynamic_cast<ISoftBodyCollisionShape*>(shape0) && dynamic_cast<BTerrain*>(shape1))
		return new ISoftConcaveAlgorithm();

	if (dynamic_cast<BMesh*>(shape0) && dynamic_cast<ISoftBodyCollisionShape*>(shape1))
		return new ISoftConcaveAlgorithm();

	if (dynamic_cast<BTerrain*>(shape0) && dynamic_cast<ISoftBodyCollisionShape*>(shape1))
		return new ISoftConcaveAlgorithm();

	if (dynamic_cast<ISoftBodyCollisionShape*>(shape0))
		return new ISoftRigidAlgorithm();

	if (dynamic_cast<ISoftBodyCollisionShape*>(shape1))
		return new ISoftRigidAlgorithm();

	if (shape0->mered("BGridPlane") || shape1->mered("BGridPlane"))
		return new IConvexPlaneAlgorithm();

	if (shape0->mered("BSurface") || shape1->mered("BSurface"))
		return new IConvexConcaveAlgorithm();

	if (shape0->mered("BTerrain") || shape1->mered("BTerrain"))
		return new IConvexConcaveAlgorithm();

	if (shape0->mered("BGeometry") || shape1->mered("BGeometry"))
		return new IConvexConcaveAlgorithm();

	if (shape0->mered("BMesh") || shape1->mered("BMesh"))
		return new IConvexConcaveAlgorithm();

	return new IConvexConvexAlgorithm();
}

void member_BScene::stepSimulation(BReal stepTime)
{
	for (int i = 0; i < bodies.size(); i++)
	{
		BBody* body = bodies[i];
		if (!object_member(body)->enabled)
			continue;
		if (body_member(body)->dirty)
		{
			body_member(body)->updateInertia();
			body_member(body)->dirty = false;
		}
		if (body_member(body)->state != BBody::State_Sleeping && body_member(body)->mass > REAL_EPSILON)
		{
			body_member(body)->updateSolverData(stepTime);
			const BMatrix& matrix = node_member(body)->matrix;
			const BVector& linearVelocity = body_member(body)->linearVelocity;
			const BVector& angularVelocity = body_member(body)->angularVelocity;
			body_member(body)->predictedMatrix = IntegrateTransform(matrix, linearVelocity, angularVelocity, stepTime);
		}
		broadphase.updateProxy(body);
	}
	for (int i = 0; i < softBodies.size(); ++i)
	{
		BSoftBody* softBody = softBodies[i];
		if (softBody->state() != BBody::State_Sleeping && body_member(softBody)->mass > REAL_EPSILON)
		{
			softBody->predictMotion(stepTime);
		}
	}

	algorithms.reset();
	for (int i = 0; i < broadphase.overlapPairCount(); i++)
	{
		IOverlapPair& pair = broadphase.overlapPair(i);
		BBody* bodyA = pair.proxy0->body;
		BBody* bodyB = pair.proxy1->body;
		if (!this->needsCollision(bodyA, bodyB))
			continue;
		for (int ia = 0; ia < bodyA->shapeCount(); ia++)
		{
			BShape* shapeA = bodyA->shape(ia);
			if (!shape_member(shapeA)->colliding)
				continue;
			if (shape_member(shapeA)->proxy)
				shapeA = shape_member(shapeA)->proxy;
			BMatrix shapeTransA = shapeA->matrix() * bodyA->matrix();
			BSpace spaceA = shape_member(shapeA)->transSpace(shapeTransA);
			for (int ib = 0; ib < bodyB->shapeCount(); ib++)
			{
				BShape* shapeB = bodyB->shape(ib);
				const BMatrix& matrix = shapeB->matrix() * bodyB->matrix();
				BSpace space = shape_member(shapeB)->transSpace(matrix);
				if (!shape_member(shapeB)->colliding)
					continue;
				if (shape_member(shapeB)->proxy)
					shapeB = shape_member(shapeB)->proxy;
				BMatrix shapeTransB = shapeB->matrix() * bodyB->matrix();
				BSpace spaceB = shape_member(shapeB)->transSpace(shapeTransB);
				if (spaceB.intersect(spaceA))
				{
					if (IAlgorithm* algorithm = this->createAlgorithm(shapeA, shapeB))
					{
						algorithm->setBodyA(bodyA, shapeA, shapeTransA);
						algorithm->setBodyB(bodyB, shapeB, shapeTransB);
						algorithms.append(algorithm);
					}
				}
			}
		}
	}
	for (int i = 0; i < algorithms.size(); i++)
	{
		IAlgorithm* algorithm = algorithms[i];
		BBody* bodyA = algorithm->_bodyA;
		BBody* bodyB = algorithm->_bodyB;
		//if (body_member(bodyA)->continuous || body_member(bodyB)->continuous)
		//{
		//	algorithm->timeImpact();
		//}
		//else
		{
			algorithm->caculate();
		}
	}

	contacts.reset();
	for (int i = 0; i < algorithms.size(); i++)
	{
		IAlgorithm* algorithm = algorithms[i];
		if (algorithm->numContact == 0)
			continue;
		BBody* bodyA = algorithm->_bodyA;
		BBody* bodyB = algorithm->_bodyB;
		SolverData& dataA = body_member(bodyA)->solverData;
		SolverData& dataB = body_member(bodyB)->solverData;
		for (int j = 0; j < algorithm->numContact; j++)
		{
			BContact& contact = algorithm->contacts[j];
			contact.bodyA = bodyA;
			contact.bodyB = bodyB;
			contacts.append(contact);
			const BString& nameA = bodyA->name();
			const BString& nameB = bodyB->name();
			if (!bodyA->contact(contact))
				continue;
			if (!bodyB->contact(contact))
				continue;

			setupContact(contact, stepTime);
		}
	}
	for (int i = 0; i < restraints.size(); i++)
	{
		BRestraint* restraint = restraints[i];
		BBody* bodyA = restraint_member(restraint)->bodyA;
		BBody* bodyB = restraint_member(restraint)->bodyB;
		if (!bodyA || !bodyB)
			continue;
		if (bodyA == bodyB)
			continue;
		if (!object_member(restraint)->enabled)
			continue;
		bool validA = body_member(bodyA)->mass > REAL_EPSILON;
		bool validB = body_member(bodyB)->mass > REAL_EPSILON;
		if (validA || validB)
		{
			validRestraints.insert(restraint);
		}
	}
	for (int i = 0; i < groups.size(); i++)
	{
		selectRestraints(groups[i]);
	}
	solveRestraints(stepTime);

	for (int i = 0; i < bodies.size(); i++)
	{
		BBody* body = bodies[i];
		if (body_member(body)->mass <= REAL_EPSILON)
			continue;
		SolverData& data = body_member(body)->solverData;
		if (data.pushVelocity.valid() || data.turnVelocity.valid())
		{
			BMatrix newMatrix = IntegrateTransform(body->matrix(), data.pushVelocity, data.turnVelocity * 0.2, stepTime);
			body->setMatrix(newMatrix);
		}
		body_member(body)->linearVelocity += data.linearVelocity + data.forceVelocity;
		body_member(body)->angularVelocity += data.angularVelocity + data.torqueVelocity;
		body_member(body)->updateDeactivating(stepTime);
		if (body_member(body)->state != BBody::State_Sleeping)
		{
			const BVector& linearVelocity = body_member(body)->linearVelocity;
			const BVector& angularVelocity = body_member(body)->angularVelocity;
			BMatrix newMatrix = IntegrateTransform(body->matrix(), linearVelocity, angularVelocity, stepTime);
			body->setMatrix(newMatrix);
		}
	}
	if (softBodies.size())
	{
		int count = 0;
		for (int i = 0; i < softBodies.size(); ++i)
		{
			BSoftBody* softBody = softBodies[i];
			count = bMax(count, softbody_member(softBody)->clusterSolverCount);
		}
		for (int i = 0; i < softBodies.size(); ++i)
		{
			softBodies[i]->prepareClusters(count);
		}
		for (int i = 0; i < count; ++i)
		{
			for (int j = 0; j < softBodies.size(); ++j)
			{
				softBodies[j]->solveClusters(1);
			}
		}
		for (int i = 0; i < softBodies.size(); ++i)
		{
			BSoftBody* softBody = softBodies[i];
			softBody->collisionSoftBody(softBody);
			softBody->cleanupClusters();
			if (softBody->state() != BBody::State_Sleeping)
			{
				softBody->applyClusters(false);
				for (int i = 0; i < 1; ++i)
				{
					softBody->solveAnchors();
					softBody->solveContacts();
					softBody->solveLinks();
				}
				softBody->applyVelocity();
				softBody->dampClusters();
				softBody->applyClusters(true);

				softBody->applyRepulsionForce(stepTime, true);
				softBody->updateNormals();
				softBody->updatePoints();
			}
		}
	}
}
bool member_BScene::needsCollision(const BBody* bodyA, const BBody* bodyB)
{
	if (!object_member(bodyA)->enabled || !object_member(bodyB)->enabled)
		return false;
	if (node_member(bodyA)->shapes.empty() || node_member(bodyB)->shapes.empty())
		return false;
	if (body_member(bodyA)->mass < REAL_EPSILON && body_member(bodyB)->mass < REAL_EPSILON)
		return false;
	if (bodyA->state() == BBody::State_Sleeping && bodyB->state() == BBody::State_Sleeping)
		return false;
	for (int i = 0; i < body_member(bodyA)->restraints.size(); i++)
	{
		const BRestraint* restraint = body_member(bodyA)->restraints[i];
		if (restraint_member(restraint)->colliding)
			continue;
		if (restraint_member(restraint)->bodyA == bodyB || restraint_member(restraint)->bodyB == bodyB)
			return false;
	}
	return true;
}
void member_BScene::selectRestraints(BGroup* group)
{
	for (int i = 0; i < group_member(group)->restraints.size(); i++)
	{
		BRestraint* restraint = group_member(group)->restraints[i];
		BBody* bodyA = restraint_member(restraint)->bodyA;
		BBody* bodyB = restraint_member(restraint)->bodyB;
		if (!bodyA || !bodyB)
			continue;
		if (bodyA == bodyB)
			continue;
		if (!object_member(restraint)->enabled)
			continue;
		bool validA = body_member(bodyA)->mass > REAL_EPSILON;
		bool validB = body_member(bodyB)->mass > REAL_EPSILON;
		if (validA || validB)
		{
			validRestraints.insert(restraint);
		}
	}
	for (int i = 0; i < group_member(group)->children.size(); i++)
	{
		BGroup* child = group_member(group)->children[i];
		selectRestraints(child);
	}
}
void member_BScene::solveRestraints(BReal stepTime)
{
	prepareFixedRestraints();
	int row = 0;
	for (int i = 0; i < validRestraints.size(); i++)
	{
		BRestraint* restraint = validRestraints[i];
		int size = restraint->solverSize();
		if (size > 0)
		{
			restraintSolverItems.append(BSolverItem(), size);
			BSolverItem* solverItems = restraintSolverItems.data(row);
			setupRestraint(solverItems, size, restraint, stepTime);
		}
		row += size;
	}

	if (contactSolverItems.size())
	{
		int iterations = 0;
		while (iterations++ < 20)
		{
			BReal leastResidual = 0;
			for (int i = 0; i < contactSolverItems.size(); i++)
			{
				BSolverItem& solverItem = contactSolverItems[i];
				if (solverItem.penetrationImpulse > REAL_EPSILON)
				{
					BReal residual = solveSinglePenetration(solverItem);
					leastResidual = bMax(leastResidual, fabs(residual));
				}
			}
			if (leastResidual < REAL_EPSILON)
				break;
		}
	}

	int iterations = 0;
	while (iterations++ < 20)
	{
		BReal leastResidual = 0;

		for (int i = 0; i < restraintSolverItems.size(); i++)
		{
			BSolverItem& solverItem = restraintSolverItems[i];
			BReal residual = solveSingleRestraint(solverItem);
			leastResidual = bMax(leastResidual, fabs(residual));
		}

		for (int i = 0; i < contactSolverItems.size(); i++)
		{
			BSolverItem& solverItem = contactSolverItems[i];
			BReal residual = solveSingleRestraint(solverItem);
			leastResidual = bMax(leastResidual, fabs(residual));
		}

		for (int i = 0; i < frictionSolverItems.size(); i++)
		{
			BSolverItem& item = frictionSolverItems[i];
			if (item.contactIndex < 0)
				continue;
			BReal appliedImpulse = contactSolverItems[item.contactIndex].appliedImpulse;
			if (appliedImpulse <= 0)
				continue;
			item.impulseLowerLimit = -item.friction * appliedImpulse;
			item.impulseUpperLimit = item.friction * appliedImpulse;
			BReal residual = solveSingleRestraint(item);
			leastResidual = bMax(leastResidual, fabs(residual));
		}

		for (int i = 0; i < rollingFrictionSolverItems.size(); i++)
		{
			BSolverItem& item = rollingFrictionSolverItems[i];
			if (item.contactIndex < 0)
				continue;
			BReal appliedImpulse = contactSolverItems[item.contactIndex].appliedImpulse;
			if (appliedImpulse <= 0)
				continue;
			item.impulseLowerLimit = -item.friction * appliedImpulse;
			item.impulseUpperLimit = item.friction * appliedImpulse;
			BReal residual = solveSingleRestraint(item);
			leastResidual = bMax(leastResidual, fabs(residual));
		}

		if (leastResidual < REAL_EPSILON)
			break;
	}

	for (int i = 0; i < restraintSolverItems.size(); i++)
	{
		const BSolverItem& item = restraintSolverItems[i];
		if (BRestraint* restraint = item.restraint)
		{
			if (abs(item.appliedImpulse) >= restraint->breakImpulse())
			{
				restraint->setEnabled(false);
			}
		}
	}

	validRestraints.reset();
	restraintSolverItems.reset();
	contactSolverItems.reset();
	frictionSolverItems.reset();
	rollingFrictionSolverItems.reset();
}

void member_BScene::prepareFixedRestraints()
{
	fixedInfoMap.reset();
	for (int i = 0; i < validRestraints.size(); i++)
	{
		BRestraint* restraint = validRestraints[i];
		if (BFixedRestraint* fixedRestraint = dynamic_cast<BFixedRestraint*>(restraint))
		{
			BBody* bodyA = restraint_member(fixedRestraint)->bodyA;
			BBody* bodyB = restraint_member(fixedRestraint)->bodyB;
			BReal massA = body_member(bodyA)->mass;
			BReal massB = body_member(bodyB)->mass;
			BReal mass = massA + massB;
			if (massA == 0 || massB == 0)
				mass = 0;
			BReal invMass = 0;
			BVector inertia;
			BVector invInertia;
			if (mass > REAL_EPSILON)
			{
				invMass = BReal(1) / mass;
				BVector center = (bodyB->position() + bodyA->position()) / 2;
				BMatrix matrixA = bodyA->matrix();
				BMatrix matrixB = bodyB->matrix();
				matrixA.setPosition(matrixA.position() - center);
				matrixB.setPosition(matrixB.position() - center);
				BBox box = bodyA->box() * matrixA;
				box.expand(bodyB->box() * matrixB);
				fixedRestraint->setProperty("center", center);
				fixedRestraint->setProperty("box", box);

				BReal lx = 2 * box.lena();
				BReal ly = 2 * box.lenb();
				BReal lz = 2 * box.lenc();

				inertia.x() = mass / 12 * (ly * ly + lz * lz);
				inertia.y() = mass / 12 * (lx * lx + lz * lz);
				inertia.z() = mass / 12 * (lx * lx + ly * ly);

				invInertia.x() = inertia.x() > REAL_EPSILON ? 1 / inertia.x() : 0;
				invInertia.y() = inertia.y() > REAL_EPSILON ? 1 / inertia.y() : 0;
				invInertia.z() = inertia.z() > REAL_EPSILON ? 1 / inertia.z() : 0;
			}

			FixedInfo& infoA = fixedInfoMap[bodyA];
			infoA.mass = mass;
			infoA.invMass = invMass;
			infoA.inertia = inertia;
			infoA.invInertia = invInertia;
			FixedInfo& infoB = fixedInfoMap[bodyB];
			infoB.mass = mass;
			infoB.invMass = invMass;
			infoB.inertia = inertia;
			infoB.invInertia = invInertia;
		}
	}
}
void member_BScene::updateFixedInfo(BSolverItem& item)
{
	BBody* bodyA = item.bodyA();
	BBody* bodyB = item.bodyB();

	auto infoA = fixedInfoMap.find(bodyA);
	auto infoB = fixedInfoMap.find(bodyB);
	if (infoA.valid() && infoB.valid())
	{
		item.massA = body_member(bodyA)->mass;
		item.invMassA = body_member(bodyA)->invMass;
		item.invInertiaA = body_member(bodyA)->invInertia;
		item.massB = body_member(bodyB)->mass;
		item.invMassB = body_member(bodyB)->invMass;
		item.invInertiaB = body_member(bodyB)->invInertia;
	}
	else
	{
		if (infoA.valid())
		{
			item.massA = infoA->mass;
			item.invMassA = infoA->invMass;
			item.invInertiaA = infoA->invInertia;
		}
		else
		{
			item.massA = body_member(bodyA)->mass;
			item.invMassA = body_member(bodyA)->invMass;
			item.invInertiaA = body_member(bodyA)->invInertia;
		}
		if (infoB.valid())
		{
			item.massB = infoB->mass;
			item.invMassB = infoB->invMass;
			item.invInertiaB = infoB->invInertia;
		}
		else
		{
			item.massB = body_member(bodyB)->mass;
			item.invMassB = body_member(bodyB)->invMass;
			item.invInertiaB = body_member(bodyB)->invInertia;
		}
	}
}
void member_BScene::setupRestraint(BSolverItem* items, int solverSize, BRestraint* restraint, BReal stepTime)
{
	BBody* bodyA = restraint_member(restraint)->bodyA;
	BBody* bodyB = restraint_member(restraint)->bodyB;

	SolverData& dataA = body_member(bodyA)->solverData;
	SolverData& dataB = body_member(bodyB)->solverData;

	for (int i = 0; i < solverSize; i++)
	{
		BSolverItem& item = items[i];
		item.restraint = restraint;
		updateFixedInfo(item);
	}

	restraint->soverItems(items, stepTime);

	for (int i = 0; i < solverSize; i++)
	{
		BSolverItem& item = items[i];

		item.angularComponentA = item.angularAccA(item.torqueAxisA);
		item.angularComponentB = item.angularAccB(item.torqueAxisB);

		BReal breakingImpulse = restraint->breakImpulse();
		if (item.impulseLowerLimit < -breakingImpulse)
			item.impulseLowerLimit = -breakingImpulse;
		if (item.impulseUpperLimit > breakingImpulse)
			item.impulseUpperLimit = breakingImpulse;

		BVector linearA = item.contactNormalA * item.invMassA;
		BVector linearB = item.contactNormalB * item.invMassB;

		item.diagABInv = 0;
		BReal sum = 0;
		sum += linearA.dot(item.contactNormalA);
		sum += item.angularComponentA.dot(item.torqueAxisA);
		sum += linearB.dot(item.contactNormalB);
		sum += item.angularComponentB.dot(item.torqueAxisB);
		if (sum < -DBL_EPSILON || sum > DBL_EPSILON)
		{
			item.diagABInv = 1 / sum;
		}

		BVector linearVelocityA = body_member(bodyA)->linearVelocity + dataA.forceVelocity;
		BVector angularVelocityA = body_member(bodyA)->angularVelocity + dataA.torqueVelocity;
		BVector linearVelocityB = body_member(bodyB)->linearVelocity + dataB.forceVelocity;
		BVector angularVelocityB = body_member(bodyB)->angularVelocity + dataB.torqueVelocity;

		BReal vel1Dotn = item.contactNormalA.dot(linearVelocityA) + item.torqueAxisA.dot(angularVelocityA);
		BReal vel2Dotn = item.contactNormalB.dot(linearVelocityB) + item.torqueAxisB.dot(angularVelocityB);

		item.restraintImpulse = (item.velocity - (vel1Dotn + vel2Dotn)) * item.diagABInv;
	}
}
void member_BScene::setupContact(BContact& contact, BReal stepTime)
{
	BBody* bodyA = contact.bodyA;
	BBody* bodyB = contact.bodyB;

	BVector localPointA = contact.pointA - bodyA->position();
	BVector localPointB = contact.pointB - bodyB->position();

	SolverData& dataA = body_member(bodyA)->solverData;
	SolverData& dataB = body_member(bodyB)->solverData;

	int contactIndex = contactSolverItems.size();
	BSolverItem& item = contactSolverItems.append();

	item.contact = &contact;
	updateFixedInfo(item);

	BVector torqueAxisA = localPointA.cross(contact.normalB);
	BVector torqueAxisB = localPointB.cross(contact.normalA);

	item.angularComponentA = item.angularAccA(torqueAxisA);
	item.angularComponentB = item.angularAccB(torqueAxisB);

	BVector vecA = (item.angularComponentA).cross(localPointA);
	BReal denomA = item.invMassA + contact.normalB.dot(vecA);
	BVector vecB = (-item.angularComponentB).cross(localPointB);
	BReal denomB = item.invMassB + contact.normalB.dot(vecB);
	BReal sum = denomA + denomB;
	item.diagABInv = bZero(sum) ? 0 : 1 / sum;

	item.contactNormalA = contact.normalB;
	item.torqueAxisA = torqueAxisA;

	item.contactNormalB = contact.normalA;
	item.torqueAxisB = torqueAxisB;

	item.friction = contact.friction;

	if (BBody* body = dataA.body)
	{
		dataA.linearVelocity += (item.contactNormalA * item.invMassA) * item.appliedImpulse;
		dataA.angularVelocity += item.angularComponentA * item.appliedImpulse;
	}

	if (BBody* body = dataB.body)
	{
		dataB.linearVelocity += (-item.contactNormalB * item.invMassB) * (-item.appliedImpulse);
		dataB.angularVelocity += (-item.angularComponentB) * (-item.appliedImpulse);
	}

	BVector linearVelocityA = body_member(bodyA)->linearVelocity;// +dataA.forceVelocity;
	BVector angularVelocityA = body_member(bodyA)->angularVelocity;// + dataA.torqueVelocity;
	BVector linearVelocityB = body_member(bodyB)->linearVelocity;// + dataB.forceVelocity;
	BVector angularVelocityB = body_member(bodyB)->angularVelocity;// + dataB.torqueVelocity;

	BVector vel1 = bodyA->velocityInPoint(localPointA);
	BVector vel2 = bodyB->velocityInPoint(localPointB);
	BReal rel_vel = contact.normalB.dot(vel2 - vel1);
	BReal restitution = contact.restitution * rel_vel;
	if (restitution <= 0)
	{
		restitution = 0;
	}

	BReal vel1Dotn = item.contactNormalA.dot(linearVelocityA) + item.torqueAxisA.dot(angularVelocityA);
	BReal vel2Dotn = item.contactNormalB.dot(linearVelocityB) + item.torqueAxisB.dot(angularVelocityB);

	BReal positionalError = 0;
	BReal velocityError = restitution - (vel1Dotn + vel2Dotn);
	if (contact.distance > 0 && contact.fraction > REAL_EPSILON)
	{
		velocityError -= contact.distance / stepTime * contact.fraction;
	}
	else
	{
		positionalError = -contact.distance / stepTime;
	}

	BReal penetrationImpulse = positionalError * item.diagABInv;
	BReal velocityImpulse = velocityError * item.diagABInv;
	if (contact.distance > REAL_EPSILON)
	{
		item.restraintImpulse = penetrationImpulse + velocityImpulse;
		item.penetrationImpulse = 0;
	}
	else
	{
		item.restraintImpulse = velocityImpulse;
		item.penetrationImpulse = penetrationImpulse;
	}
	item.impulseLowerLimit = 0;
	item.impulseUpperLimit = BReal(1e10);

	BVector velocity = vel2 - vel1;
	//item.contactIndex = contactIndex;
	if (contact.rollingFriction > REAL_EPSILON)
	{
		//addRollingFrictionRestraint(&contact, contact.normalB, contactIndex);
		//if (contact.rollingFrictionDir1.length() > 0.000001)
		//	addRollingFrictionRestraint(&contact, contact.rollingFrictionDir1, contactIndex);
		//if (contact.rollingFrictionDir2.length() > 0.000001)
		//	addRollingFrictionRestraint(&contact, contact.rollingFrictionDir2, contactIndex);
	}

	BVector lateralFrictionDir1 = velocity - contact.normalB * rel_vel;
	BReal lateralFrictionLen2 = lateralFrictionDir1.length2();
	if (lateralFrictionLen2 > REAL_EPSILON)
	{
		lateralFrictionDir1 *= BReal(1) / sqrt(lateralFrictionLen2);
		addFrictionRestraint(contact, lateralFrictionDir1, contactIndex, localPointA, localPointB, stepTime);
	}
	else
	{
		BVector lateralFrictionDir2;
		PlaneSpace(contact.normalB, lateralFrictionDir1, lateralFrictionDir2);
		addFrictionRestraint(contact, lateralFrictionDir1, contactIndex, localPointA, localPointB, stepTime);
	}

}
void member_BScene::addFrictionRestraint(BContact& contact, const BVector& normalAxis, int contactIndex, const BVector& localPointA, const BVector& localPointB, BReal stepTime)
{
	BBody* bodyA = contact.bodyA;
	BBody* bodyB = contact.bodyB;
	SolverData& dataA = body_member(bodyA)->solverData;
	SolverData& dataB = body_member(bodyB)->solverData;

	BSolverItem& item = frictionSolverItems.append();
	item.contactIndex = contactIndex;
	item.friction = contact.friction;
	item.contact = &contact;

	updateFixedInfo(item);

	item.contactNormalA = normalAxis;
	item.torqueAxisA = localPointA.cross(item.contactNormalA);
	item.angularComponentA = item.angularAccA(item.torqueAxisA);

	item.contactNormalB = -normalAxis;
	item.torqueAxisB = localPointB.cross(item.contactNormalB);
	item.angularComponentB = item.angularAccB(item.torqueAxisB);

	BVector vecA = (item.angularComponentA).cross(localPointA);
	BReal denomA = item.invMassA + normalAxis.dot(vecA);
	BVector vecB = (-item.angularComponentB).cross(localPointB);
	BReal denomB = item.invMassB + normalAxis.dot(vecB);
	BReal sum = denomA + denomB;
	item.diagABInv = bZero(sum) ? 0 : 1 / sum;

	BVector linearVelocityA = body_member(bodyA)->linearVelocity;// + dataA.forceVelocity;
	BVector angularVelocityA = body_member(bodyA)->angularVelocity;
	BVector linearVelocityB = body_member(bodyB)->linearVelocity;// + dataB.forceVelocity;
	BVector angularVelocityB = body_member(bodyB)->angularVelocity;

	BReal vel1Dotn = item.contactNormalA.dot(linearVelocityA) + item.torqueAxisA.dot(angularVelocityA);
	BReal vel2Dotn = item.contactNormalB.dot(linearVelocityB) + item.torqueAxisB.dot(angularVelocityB);

	BReal distance = (contact.pointA - contact.pointB).dot(normalAxis);
	BReal positionError = -distance / stepTime;

	item.restraintImpulse = (positionError - (vel1Dotn + vel2Dotn)) * item.diagABInv;
}
void member_BScene::addRollingFrictionRestraint(BContact& contact, const BVector& normalAxis, int contactIndex)
{
	BBody* bodyA = contact.bodyA;
	BBody* bodyB = contact.bodyB;
	SolverData& dataA = body_member(bodyA)->solverData;
	SolverData& dataB = body_member(bodyB)->solverData;

	BSolverItem& item = rollingFrictionSolverItems.append();
	item.contact = &contact;
	item.contactIndex = contactIndex;
	item.contactNormalA = normalAxis;
	item.contactNormalB = -normalAxis;
	item.friction = contact.rollingFriction;

	updateFixedInfo(item);

	item.torqueAxisA = -normalAxis;
	item.angularComponentA = item.angularAccA(-normalAxis);
	item.torqueAxisB = normalAxis;
	item.angularComponentB = item.angularAccB(normalAxis);

	BReal sum = 0;
	sum += item.angularComponentA.dot(item.torqueAxisA);
	sum += item.angularComponentB.dot(item.torqueAxisB);
	item.diagABInv = bZero(sum) ? 0 : 1 / sum;

	BVector linearVelocityA = body_member(bodyA)->linearVelocity;// + dataA.forceVelocity;
	BVector angularVelocityA = body_member(bodyA)->angularVelocity;
	BVector linearVelocityB = body_member(bodyB)->linearVelocity;// + dataB.forceVelocity;
	BVector angularVelocityB = body_member(bodyB)->angularVelocity;

	BReal vel1Dotn = item.contactNormalA.dot(linearVelocityA) + item.torqueAxisA.dot(angularVelocityA);
	BReal vel2Dotn = item.contactNormalB.dot(linearVelocityB) + item.torqueAxisB.dot(angularVelocityB);

	item.restraintImpulse = -(vel1Dotn + vel2Dotn) * item.diagABInv;
}

BReal member_BScene::solveSingleRestraint(BSolverItem& item)
{
	SolverData& dataA = body_member(item.bodyA())->solverData;
	SolverData& dataB = body_member(item.bodyB())->solverData;

	const BReal deltaVel1Dotn = item.contactNormalA.dot(dataA.linearVelocity) + item.torqueAxisA.dot(dataA.angularVelocity);
	const BReal deltaVel2Dotn = item.contactNormalB.dot(dataB.linearVelocity) + item.torqueAxisB.dot(dataB.angularVelocity);

	BReal deltaImpulse = item.restraintImpulse;
	deltaImpulse -= deltaVel1Dotn * item.diagABInv;
	deltaImpulse -= deltaVel2Dotn * item.diagABInv;

	const BReal sumImpulse = item.appliedImpulse + deltaImpulse;
	if (sumImpulse < item.impulseLowerLimit)
	{
		deltaImpulse = item.impulseLowerLimit - item.appliedImpulse;
		item.appliedImpulse = item.impulseLowerLimit;
	}
	else if (sumImpulse > item.impulseUpperLimit)
	{
		deltaImpulse = item.impulseUpperLimit - item.appliedImpulse;
		item.appliedImpulse = item.impulseUpperLimit;
	}
	else
	{
		item.appliedImpulse = sumImpulse;
	}

	if (BBody* body = dataA.body)
	{
		dataA.linearVelocity += (item.contactNormalA * item.invMassA) * deltaImpulse;
		dataA.angularVelocity += item.angularComponentA * deltaImpulse;
	}

	if (BBody* body = dataB.body)
	{
		dataB.linearVelocity += (item.contactNormalB * item.invMassB) * deltaImpulse;
		dataB.angularVelocity += item.angularComponentB * deltaImpulse;
	}

	return deltaImpulse * (1 / item.diagABInv);
}
BReal member_BScene::solveSinglePenetration(BSolverItem& item)
{
	SolverData& dataA = body_member(item.bodyA())->solverData;
	SolverData& dataB = body_member(item.bodyB())->solverData;

	const BReal deltaVel1Dotn = item.contactNormalA.dot(dataA.pushVelocity) + item.torqueAxisA.dot(dataA.pushVelocity);
	const BReal deltaVel2Dotn = item.contactNormalB.dot(dataB.pushVelocity) + item.torqueAxisB.dot(dataB.pushVelocity);
	BReal deltaImpulse = item.penetrationImpulse - deltaVel1Dotn * item.diagABInv - deltaVel2Dotn * item.diagABInv;

	BReal sumImpulse = item.appliedPushImpulse + deltaImpulse;
	if (sumImpulse < item.impulseLowerLimit)
	{
		deltaImpulse = item.impulseLowerLimit - item.appliedPushImpulse;
		item.appliedPushImpulse = item.impulseLowerLimit;
	}
	else if (sumImpulse > item.impulseUpperLimit)
	{
		deltaImpulse = item.impulseUpperLimit - item.appliedImpulse;
		item.appliedImpulse = item.impulseUpperLimit;
	}
	else
	{
		item.appliedPushImpulse = sumImpulse;
	}

	if (BBody* body = dataA.body)
	{
		dataA.pushVelocity += item.contactNormalA * item.invMassA * deltaImpulse;
		dataA.turnVelocity += item.angularComponentA * deltaImpulse;
	}

	if (BBody* body = dataB.body)
	{
		dataB.pushVelocity += item.contactNormalB * item.invMassB * deltaImpulse;
		dataB.turnVelocity += item.angularComponentB * deltaImpulse;
	}

	return deltaImpulse * (1 / item.diagABInv);
}

void member_BScene::slotShapeFreshed(BShape* shape)
{
	refresh = true;
}
void member_BScene::slotShapeDirty(BShape* shape)
{
	if (shape_member(shape)->visible)
		refresh = true;
	dirtySpace = true;
	boss->emit(Signal_Dirty, shape);
}
void member_BScene::slotShapeTransformed(BShape* shape)
{
	if (shape_member(shape)->visible)
		refresh = true;
	if (!space.contain(shape->space()))
	{
		dirtySpace = true;
	}
	if (sceneOctTree)
	{
		SceneOctTreeItem& olditem = shapeSpaceMap[shape];
		olditem->shapes.remove(shape);

		SceneOctTreeItem item = sceneOctTree->create(shape->space());
		item->shapes.append(shape);
		shapeSpaceMap[shape] = item;
	}
	boss->emit(Signal_Transformed, shape);
}

void member_BScene::slotNodeFreshed(BNode* node)
{
	refresh = true;
}
void member_BScene::slotNodeDirty(BNode* node)
{
	refresh = true;
	dirtySpace = true;
	boss->emit(Signal_Dirty, node);
}
void member_BScene::slotNodeInsert(BNode* node, const BValue& value)
{
	boss->emit(Signal_Insert, value);
}
void member_BScene::slotNodeRemove(BNode* node, const BValue& value)
{
	boss->emit(Signal_Remove, value);
}
void member_BScene::slotNodeTransformed(BNode* node)
{
	if (node_member(node)->visible)
		refresh = true;
	if (!space.contain(node->space()))
	{
		dirtySpace = true;
		return;
	}
	if (sceneOctTree)
	{
		SceneOctTreeItem& olditem = nodeSpaceMap[node];
		olditem->nodes.remove(node);

		SceneOctTreeItem item = sceneOctTree->create(node->space());
		item->nodes.append(node);
		nodeSpaceMap[node] = item;
	}
	boss->emit(Signal_Transformed, node);
}

void member_BScene::slotGroupFreshed(BGroup* group)
{
	refresh = true;
	dirtySpace = true;
	boss->emit(Signal_Freshed, group);
}
void member_BScene::slotGroupDirty(BGroup* group)
{
	refresh = true;
	dirtySpace = true;
	boss->emit(Signal_Dirty, group);
}
void member_BScene::slotGroupInsert(BGroup* group, const BValue& value)
{
	if (BGroup* child = value)
	{
		attachGroup(child);
	}
	else if (BNode* node = value)
	{
		if (nodes.contain(node))
		{
			this->disconnect(node);
			nodes.remove(node);
			if (sceneOctTree)
			{
				SceneOctTreeItem& item = nodeSpaceMap[node];
				item->nodes.remove(node);
				nodeSpaceMap.remove(node);
			}
			boss->emit(Signal_Remove, BValue(node, this));
		}
		else if (BBody* body = value)
		{
			if (!bodies.contain(body))
			{
				if (BSoftBody* softBody = dynamic_cast<BSoftBody*>(body))
					softBodies.insert(softBody);
				broadphase.createProxy(body);
				bodies.insert(body);
			}
		}
		else if (BCamera* camera = value)
		{
			cameras.insert(camera);
		}
		else if (BLight* light = value)
		{
			lights.insert(light);
		}
	}
	refresh = true;
	dirtySpace = true;
	boss->emit(Signal_Insert, value);
}
void member_BScene::slotGroupRemove(BGroup* group, const BValue& value)
{
	if (BGroup* child = value)
	{
		detachGroup(child);
	}
	else if (BBody* body = value)
	{
		if (BSoftBody* softBody = dynamic_cast<BSoftBody*>(body))
			softBodies.remove(softBody);
		broadphase.removeProxy(body);
		bodies.remove(body);
	}
	else if (BCamera* camera = value)
	{
		cameras.remove(camera);
	}
	else if (BLight* light = value)
	{
		lights.remove(light);
	}
	refresh = true;
	dirtySpace = true;
	boss->emit(Signal_Remove, value);
}
